#pragma once

#include <memory>
#include <fstream>

#include <glog/logging.h>
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/TwistStamped.h>

#include "gps_imu_fusion/eskf.h"

class ImuGpsNode {
public:
	ImuGpsNode(ros::NodeHandle& nh);

	~ImuGpsNode();

	void ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg_ptr);

	void GpsPositionCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg_ptr);

private:
	//void PublishState(const ImuGps::State& state, const ros::Time stamp);

	bool initialized_;

	ConfigParameters config_;

	ros::Subscriber imu_sub_;
	ros::Subscriber gps_sub_;
	ros::Publisher state_pub_;

	std::ofstream file_gps_;
	std::ofstream file_state_;

	nav_msgs::Path path_fusion_;

	std::deque<ImuDataPtr> imu_buffer_;
	std::unique_ptr<ESKF> eskf_ptr_;
};
